#include "FollowSearchPathState.h"

#include "AStarSearch.h"
#include "AssignDecoyOrSniperState.h"
#include "PDController.h"

FollowSearchPathState::FollowSearchPathState(Graph* g, Vector2 start, Vector2 goal, bool makeTransition){
	_makeTransition = makeTransition;
	
	w = World::instance();
	com = Remote::instance();

	AStarSearch as;
	g->clearFlags();

	node* current = as.search(g, start, goal, true);

	base = Vector2(0,0);
 	for(int i = 0; i < 4; i ++){
		base.x += w->teams()[3].base_corner[i][0]; base.y += w->teams()[3].base_corner[i][1];
	}
	base.x /= 4.0f; base.y /= 4.0f;

	enemyFlag = Vector2(w->flags()[2].pos[0],w->flags()[2].pos[1]);

	path.push_back(goal); 
	while(current != NULL){
		path.push_back(Vector2(current->x, current->y));
		current = current->previous;
	}
	path.push_back(start);

	pos = path.size() - 2;

	cutoff = PATH_THRESHOLD;
}

FollowSearchPathState::~FollowSearchPathState(void)
{
}

Command FollowSearchPathState::GetCommand(MultiAgent* agent) {
	tank_t* tank = agent->getTank();
	float tthreshold = M_PI / 2.0f;
	double tmag = 40;
	double speed = 0;
	double dangerRadius = 400;

	float dist;

	dist = (Vector2(tank->pos[0], tank->pos[1])-enemyFlag).magnitude();

	// If we have completed the maze, transition into individual
	// roles of decoy and sniper.
	if (_makeTransition && dist < dangerRadius) {
		agent->setState(new AssignDecoyOrSniperState());
		return agent->GetCommand();
	}
	
	Vector2 trajectory = pfFieldPath(tank);

	return PDController::instance()->GetComand(tank, trajectory);
}

Vector2 FollowSearchPathState::pfFieldPath(tank_t* tank){
	Vector2 field(path[pos].x - tank->pos[0], path[pos].y - tank->pos[1]);
	Vector2 dir;

	if(pos == 0  || pos == path.size() - 1)
		return field;

	if(pos > path.size() - 2) pos = path.size() - 2;
	if(pos < 1) pos = 1;

	dir.x = path[pos].x-path[pos+1].x, dir.y = path[pos].y-path[pos+1].y;

	float dist = field.magnitude();
	float dot = Vector2::dot(dir, field);
	float angle = fabs(dir.angle() - field.angle());

	if(field.magnitude() <= cutoff || 
		(field.magnitude() > cutoff && 
		fabs(dir.angle() - field.angle()) > 45.0f*M_PI/180.0f && 
		Vector2::dot(dir.normalize(), field.normalize()) <= DOT_THRESHOLD)){

		setNextWayPoint(tank);
		field = Vector2(path[pos].x - tank->pos[0], path[pos].y - tank->pos[1]);
	}

	return field;
}

void FollowSearchPathState::setNextWayPoint(tank_t* tank) {
	float threshold = 1.0f * M_PI / 180.0f;
	int maxSteps = 30;
	int count = 0;

	float error = 0;
	float currentAngle;
	int tmpPos;
	
	Vector2 startPoint = path[pos];
	Vector2 currentPoint;
	Vector2 result;

	tmpPos = pos - 1;

	currentAngle = (path[tmpPos]-startPoint).angle();

	while(error < threshold && tmpPos > 0 && tmpPos < path.size() && count < maxSteps){
		tmpPos--;

		currentPoint = path[tmpPos];
		result = currentPoint - startPoint;
		error = fabs(currentAngle - result.angle());
		count ++;
	}

	cutoff = max(min(PATH_THRESHOLD, count * 4.0), MIN_PATH_THRESHOLD);

	pos = tmpPos ++;
}